#include <socket_handle.h>
#include <hal_tlv.h>
#include "hal_eeprom.h"
#include "hal_configer.h"
#include "cfg_drive.h"
#include <common.h>
#include "sys_log.h"
#include "hal_errorcode.h"
#include "hal_position.h"
#include "hal_move.h"
#include <stdlib.h>
#include "hal_socket.h"

extern int (*motor_func_callback_fun[FUNC_MOTOR_TYPE_COUNT_E])(struct TLVPacket *);
extern MotorMoveSt MotorMove;

int motor_func_00_fun(struct TLVPacket *packet) { return 1; };

int motor_func_01_fun(struct TLVPacket *packet)
{
    struct MotorMessageCmdSt *MotorValue = (struct MotorMessageCmdSt *)&(packet->value);

    uint8_t groupid, motor_num = 0;
    int16_t step_num = 0;

    for (int i = 1; i <= MOTOR_MAX_ERROR_CHANNEL; i++)
    {
        if (MotorValue->MotorSubValue[i - 1].MotorNum != 0)
        {
            groupid   = (MotorValue->MotorSubValue[i - 1].MotorNum - 1) % 4 + 1;
            motor_num = (MotorValue->MotorSubValue[i - 1].MotorNum - 1) / 4 + 1;

            MotorValue->MotorSubValue[i - 1].Position = hal_PositionRead(groupid, motor_num);
        }
    }
	
	SLOG_I("motor read position funcmd:%#x, motornum: %d, %d, %d, %d, position: %d, %d, %d, %d \t\r\n", 
    packet->tagcmd, 
    MotorValue->MotorSubValue[0].MotorNum, MotorValue->MotorSubValue[1].MotorNum,
    MotorValue->MotorSubValue[2].MotorNum, MotorValue->MotorSubValue[3].MotorNum,
    MotorValue->MotorSubValue[0].Position, MotorValue->MotorSubValue[1].Position,
    MotorValue->MotorSubValue[2].Position, MotorValue->MotorSubValue[3].Position);

    hal_SendPacketToUSART(packet);

    return 1;
}

int motor_func_02_fun(struct TLVPacket *packet)
{
    struct MotorMessageCmdSt *MotorValue = (struct MotorMessageCmdSt *)&(packet->value);
    
    SLOG_I("motor run position funcmd:%#x, motornum: %d, %d, %d, %d, position: %d, %d, %d, %d \t\r\n", 
    packet->tagcmd, 
    MotorValue->MotorSubValue[0].MotorNum, MotorValue->MotorSubValue[1].MotorNum,
    MotorValue->MotorSubValue[2].MotorNum, MotorValue->MotorSubValue[3].MotorNum,
    MotorValue->MotorSubValue[0].Position, MotorValue->MotorSubValue[1].Position,
    MotorValue->MotorSubValue[2].Position, MotorValue->MotorSubValue[3].Position);

    hal_SendPacketToUSART(packet);

    uint8_t groupid, motor_num = 0;
    int16_t step_num = 0;

    // 每次转动前把这个数组[电机 - 最大电流]清零
    memset(MotorMove.number, 0, sizeof(MotorMoveSt));

    for (int i = 1; i <= MOTOR_MAX_ERROR_CHANNEL; i++)
    {
        if (MotorValue->MotorSubValue[i - 1].MotorNum != 0)
        {
            MotorMove.number[i - 1] = (MotorValue->MotorSubValue[i - 1].MotorNum - 1);
            groupid    = (MotorValue->MotorSubValue[i - 1].MotorNum - 1) % 4 + 1;
            motor_num  = (MotorValue->MotorSubValue[i - 1].MotorNum - 1) / 4 + 1;

            step_num = MotorValue->MotorSubValue[i - 1].Position - hal_PositionRead(groupid, motor_num);
            //要运行的步数大于8000，或者当前位置<0，都属于非法情况
            if ((abs(step_num) > MOTOR_RUN_MAX_STEP) || ((int32_t)MotorValue->MotorSubValue[i - 1].Position < 0))
            {
                hal_ErrorCodeWrite(ERRORCODE_TYPE_MOTOR, s_Write_Value_Abnormal_Flag, groupid, ABNORMAL_STATE);
                SLOG_I("motor_func_02_fun step over 8000 or below 0 \t\r\n");
            }
            else
            {
                // 当前组别正有电机在转动，当step_num为0时，若改变电机运行状态，后面无法将状态复位
                if (step_num != 0)
                {
                    hal_ErrorCodeWrite(ERRORCODE_TYPE_MOTOR, m_Run_Abnormal_Flag, groupid, ABNORMAL_STATE);
                }

                hal_ErrorCodeWrite(ERRORCODE_TYPE_MOTOR, s_Write_Value_Abnormal_Flag, groupid, NORMAL_STATE);

                hal_DirectionWeite(groupid, motor_num, step_num > 0 ? DIRECTION_FORWARD : DIRECTION_BACKWARD);
                hal_StepNumberWrite(groupid, motor_num, abs(step_num));

                // 再设置电机号
                hal_MotorNumWrite(groupid, motor_num);
                //初始化AD7689
                HAL_AD7689_Init(groupid, motor_num);
            }
        }
    }

    hal_MotorMoveStartFlagSet(MOVE_STATUES_START);

    return 1;
}

int motor_func_03_fun(struct TLVPacket *packet)
{
    struct MotorMessageCmdSt *MotorValue = (struct MotorMessageCmdSt *)&(packet->value);

    SLOG_I("motor save position funcmd:%#x, motornum: %d, %d, %d, %d, position: %d, %d, %d, %d \t\r\n", 
    packet->tagcmd, 
    MotorValue->MotorSubValue[0].MotorNum, MotorValue->MotorSubValue[1].MotorNum,
    MotorValue->MotorSubValue[2].MotorNum, MotorValue->MotorSubValue[3].MotorNum,
    MotorValue->MotorSubValue[0].Position, MotorValue->MotorSubValue[1].Position,
    MotorValue->MotorSubValue[2].Position, MotorValue->MotorSubValue[3].Position);

    uint8_t groupid, motor_num = 0;
    int16_t step_num = 0;

    for (int i = 1; i <= MOTOR_MAX_ERROR_CHANNEL; i++)
    {
        if (MotorValue->MotorSubValue[i - 1].MotorNum != 0)
        {
            groupid   = (MotorValue->MotorSubValue[i - 1].MotorNum - 1) % 4 + 1;
            motor_num = (MotorValue->MotorSubValue[i - 1].MotorNum - 1) / 4 + 1;

            if ((int32_t)MotorValue->MotorSubValue[i - 1].Position < 0)
            {
                hal_ErrorCodeWrite(ERRORCODE_TYPE_MOTOR, s_Write_Value_Abnormal_Flag, groupid, ABNORMAL_STATE);
                SLOG_I("motor_func_03_fun save position below 0 \t\r\n");
            }
            else
            {
                hal_ErrorCodeWrite(ERRORCODE_TYPE_MOTOR, s_Write_Value_Abnormal_Flag, groupid, NORMAL_STATE);
                hal_PositionWrite(groupid, motor_num, MotorValue->MotorSubValue[i - 1].Position);
                hal_SingleMotorEEPromInformWrite(groupid, motor_num, EEPSTGTYPE_POSITION);
            }
        }
    }

    hal_SendPacketToUSART(packet);

    return 1;
}

int motor_func_04_fun(struct TLVPacket *packet)
{
    struct MotorVersionCmdSt *MotorVersionCmd = (struct MotorVersionCmdSt *)&(packet->value);
    MotorVersionCmd->Version                  = DEVICE_VERSION;

    SLOG_I("motor read vension funcmd:%#x, vension:%#x \t\r\n", packet->tagcmd, (uint16_t)DEVICE_VERSION);
    hal_SendPacketToUSART(packet);

    return 1;
}

int motor_func_05_fun(struct TLVPacket *packet)
{
    // 把电机的所有使能开关全部关闭
    for (int i = 1; i <= MOTOR_MAX_ERROR_CHANNEL; i++)
    {
        hal_MotorNumWrite(i, i);
    }
    hal_MotorStepCtrlDriveSet(DRIVE_STATUES_CLOSE);

    SLOG_I("motor reset funcmd:%#x, motor driver is close \t\r\n", packet->tagcmd);

    HAL_NVIC_SystemReset();

    return 1;
}

int motor_func_06_fun(struct TLVPacket *packet)
{
    struct MotorPollCmdSt *MotorPollCmd = (struct MotorPollCmdSt *)&(packet->value);

    for (int groupid = 1; groupid <= MOTOR_MAX_ERROR_CHANNEL; groupid++)
    {
        hal_ErrorcodeRead(ERRORCODE_TYPE_MOTOR, groupid, &MotorPollCmd->ErrorCode[groupid - 1]);
    }

    SLOG_I("motor poll funcmd:%#x, errorcode:%#x, %#x, %#x, %#x\t\r\n", 
    packet->tagcmd, MotorPollCmd->ErrorCode[0], MotorPollCmd->ErrorCode[1], MotorPollCmd->ErrorCode[2], MotorPollCmd->ErrorCode[3]);
    
    hal_SendPacketToUSART(packet);

    return 1;
}

int motor_func_07_fun(struct TLVPacket *packet)
{
    struct MotorRefreshCodeCmdSt *MotorRefreshCodeCmd = (struct MotorRefreshCodeCmdSt *)&(packet->value);

    hal_ErrorcodeReset();

    SLOG_I("motor errorcode refresh funcmd:%#x \t\r\n", packet->tagcmd);

    hal_SendPacketToUSART(packet);

    return 1;
}

int motor_func_08_fun(struct TLVPacket *packet)
{
    struct MotorParameterCmdSt *MotorParameterCmd = (struct MotorParameterCmdSt *)&(packet->value);

    strcpy(MotorParameterCmd->name, "motor_ch32");
    MotorParameterCmd->max_change_steps = MOTOR_RUN_MAX_STEP;
    MotorParameterCmd->current_limit = MOTOR_CURRENT_LIMIT;
    MotorParameterCmd->max_freq = FRE_MAX;
    MotorParameterCmd->min_freq = FRE_MIN;
    MotorParameterCmd->flexible_freq = FRE_FLEXIBLE;

    SLOG_I("motor parameter funcmd:%#x, typename: %s, max_change_steps: %d, current_limit: %d, fre_limit: %d, %d, flexible_freq: %d \t\r\n", packet->tagcmd, MotorParameterCmd->name,
    MOTOR_RUN_MAX_STEP, MOTOR_CURRENT_LIMIT, FRE_MAX, FRE_MIN, FRE_FLEXIBLE);

    hal_SendPacketToUSART(packet);

    return 1;
}

int (*motor_func_callback_fun[FUNC_MOTOR_TYPE_COUNT_E])(struct TLVPacket *) = {motor_func_00_fun, motor_func_01_fun, motor_func_02_fun, motor_func_03_fun,
                                                                               motor_func_04_fun, motor_func_05_fun, motor_func_06_fun, motor_func_07_fun,
                                                                               motor_func_08_fun};
